# pragma once

# include "cpp_tools.h"
# include "udpSettings.h"
# include "KM_base.h"
# include "KM_VESC_current.h"
# include "estimator_UWB_IMU.h"
# include "estimator_RTK_IMU.h"
# include "mpc_controller.h"
# include "mpc_v2s.h"
# include "mpc_v2m.h"
# include "smc_controller.h"
# include "demarcate_controller.h"

# define estimator estimator_RTK_IMU
# define tracking_controller mpc_controller
# define chassis_controller KM_VESC_current

# ifndef STATE_COR
# define STATE_COR(state) \
    int i = 0; \
    double x = state(i++); \
    double y = state(i++); \
    double varphi = state(i++); \
    double psi = state(i++); \
    double gamma = state(i++); \
    double v_x = state(i++); \
    double v_y = state(i++); \
    double omega = state(i++); \
    double a_x = state(i++); \
    double a_y = state(i++); \
    \
    /* 为了beta的稳定性，需要对v_y进行cutoff */ \
    double v = std::sqrt(std::pow(v_x,2) + std::pow(v_y,2)); \
    double v_cor = v; \
    double beta = 0; \
    double v_min = 1; /* 限制一下给控制器的最小速度（控制器启动有困难） */ \
    if (std::abs(v_cor) <= v_min) \
    { \
        v_cor = v_min; \
        beta = 0; \
    } \
    else \
    { \
        beta = std::atan2( \
            cpp_tools::cutoff(v_y,0.05/cpp_tools::saturation(std::pow(v,2),0.01,1)), \
            v_x \
        ); \
    } \
    \
    state << x,y,varphi,psi,gamma,v_cor,beta,omega,a_x,a_y;
# endif // STATE_COR

namespace lower
{
    class core
    {
    protected:
        const double pi = cpp_tools::pi;

        std::unique_ptr<cpp_tools::gameController> m_gameController;
        std::unique_ptr<estimator> m_stateEstimator;
        std::unique_ptr<tracking_controller> m_trackingController;
        std::unique_ptr<chassis_controller> m_outputController;

        struct status // 记录一系列状态
        {
            cpp_tools::mtDataManager<bool> controlStart = 0; // 是否开始进行控制

            cpp_tools::mtDataManager<std::pair<double,double>> controlOutput = {0,0}; // 记录当前真实输出（手柄模式默认为0）
        } m_status;
        
        cpp_tools::threadPool m_threadPool;
        
        bool isRunning = 1;

        std::string gameController_device = "/dev/input/js0";
        double timeOut = 0.2;

    public:
        core()
        {
            init();

            // ========================监听线程=========================
            std::thread t_mpcUpdate([&]() -> void {
                double t_start = cpp_tools::getCurrentTimeAsDouble();
                cpp_tools::udpVecSender<double> plotData_sender(udpSettings::IP::upper,udpSettings::PORT::upper_lower::plot,1);

                double lastUpdateTime = -1;
                while (isRunning)
                {
                    double t = m_stateEstimator.get()->getUpdateTime();

                    if (lastUpdateTime != t && t != -1) // 状态量有更新
                    {
                        // 获取控制量
                        auto [delta,F_x] = *m_status.controlOutput.read().access();

                        // 获取状态量
                        auto state = m_stateEstimator.get()->getStateAsVector();

                        // 修正速度值和侧偏角
                        STATE_COR(state);
                        
                        // 将状态量改写为适用于控制器的格式并输入到控制器中
                        if (*m_status.controlStart.read().access())
                        {
                            m_trackingController.get()->update(state,delta,F_x,t);
                        }

                        Eigen::VectorXd plotData(udpSettings::PORT::upper_lower::plot_len);
                        double computTime = m_trackingController.get()->getComputTime();
                        plotData << t - t_start,x,y,varphi,v,beta,omega,a_x,a_y,delta,F_x,computTime;

                        plotData_sender.send(plotData);

                        lastUpdateTime = t;
                    }
                    
                    std::this_thread::sleep_for(std::chrono::milliseconds(1));
                }
            });
            m_threadPool.push_back(t_mpcUpdate);

            // ========================手柄线程=========================
            // 原则：
            //  1.若手柄连接超时则全部输出0
            //  2.当且仅当手柄的D_paid上方被按下时(即D_paid_fr = 1)使用UDP收到的控制信号，否则使用手柄的控制信号
            std::thread t_output([&]() -> void {
                bool disConnectInfo = 0;
                cpp_tools::gameController::control_output2 lastInfo;
                while (isRunning)
                {
                    // 处理手柄输入
                    if (m_gameController.get()->isDeviceOK())
                    {
                        disConnectInfo = 0;

                        auto info = m_gameController.get()->get2();

                        // 底盘控制权限切换.
                        double steering = 0;
                        double drive = 0;
                        if (info.D_pad_fr == 1) // 使能模式下不处理其他指令
                        {
                            // 控制器开始执行
                            *m_status.controlStart.write().access() = 1;

                            // 获取上一个控制量
                            auto [delta_last,F_x_last] = *m_status.controlOutput.read().access();

                            // 获取状态量
                            auto state = m_stateEstimator.get()->getStateAsVector();
                            
                            // 修正状态量
                            STATE_COR(state);

                            // 获取新的控制量
                            auto [delta,F_x] = m_trackingController.get()->getControl(state, delta_last, F_x_last, cpp_tools::getCurrentTimeAsDouble());
                            *m_status.controlOutput.write().access() = {delta,F_x};

                            // 映射
                            steering = m_outputController.get()->steeringMapping(delta);
                            drive = m_outputController.get()->forceMapping(m_stateEstimator.get()->getStateAsStruct().v_x,F_x);

                            steering = std::max(std::min(steering,1.),-1.);
                            drive = std::max(std::min(drive,1.),-1.);
                        }
                        else // 只要松开使能就停下
                        {
                            *m_status.controlStart.write().access() = 0;

                            // 刷新控制器
                            if (info.D_pad_fr == -1 && lastInfo.D_pad_fr != -1)
                            {
                                m_trackingController.get()->refresh();
                            }

                            // 重新设置原点
                            if (info.R3 && ! lastInfo.R3)
                            {
                                std::cout << "重置原点" << std::endl;

                                auto state = m_stateEstimator.get()->getStateAsStruct();
                                m_stateEstimator.get()->reInitFrame(state.x,state.y);
                            }

                            // 设置底盘控制模式
                            if (info.D_pad_lr != 0 && lastInfo.D_pad_lr == 0)
                            {
                                m_outputController.get()->setMode(info.D_pad_lr);
                            }
                            
                            *m_status.controlOutput.write().access() = {0,0};

                            steering = info.LS_lr;
                            drive = -info.LT + info.RT;
                        }

                        m_outputController.get()->setOutPut(steering,drive);

                        lastInfo = info;
                    }
                    else
                    {
                        if (disConnectInfo == 0)
                        {
                            std::cout << "失去手柄连接" << std::endl;
                            
                            disConnectInfo = 1;
                        }
                    }

                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
                }
            });
            m_threadPool.push_back(t_output);
        };

        ~core()
        {
            isRunning = false;
            
            m_threadPool.joinAll();

            // 手动关闭以确保关闭顺序
            m_outputController.get()->shutdown();
            m_trackingController.get()->shutdown();
            m_stateEstimator.get()->shutdown();
            m_gameController.get()->shutdown();
        };

        /// @brief 初始化一些重要的成员
        void init()
        {
            cpp_tools::XmlParameterManager m_xml("device", host_path + settings_path + "/device/settings.xml");
            bool writeXML = false;
            if (!m_xml.getParameter("", "gameController", gameController_device))
            {
                std::cout << "手柄设备路径使用使用默认值: " << gameController_device << std::endl;
                m_xml.setParameter("", "gameController", gameController_device);

                writeXML = true;
            }
            if (writeXML)
            {
                m_xml.saveToFile(host_path + settings_path + "/device/settings.xml");
            }

            m_gameController = std::make_unique<cpp_tools::gameController>(gameController_device, timeOut);
            m_stateEstimator = std::make_unique<estimator>();
            m_outputController = std::make_unique<chassis_controller>();
            m_trackingController = std::make_unique<tracking_controller>();
        }

        core(const core&) = delete;
    };
}; // namespace lower
